#include "PPMproc.h"
#include <memory.h>
#include <assert.h>
#include <math.h>

// internal consts
#define PPM_RX_PEAKS_TO_SYNC	3		// number of measurements for frame sync
#define PPM_RX_LEVEL_THRESHOLD	10000	// signal power threshold, peak-to-peak / 2
#define PPM_RX_FRAME_TIMEOUT	30		// frame timeout, in multiples of the shortest peak
#define PPM_RX_FRAME_GAP		5		// interframe gap, in multiples of the shortest peak
#define PPM_RX_INPUT_AVG		14		// input metrics averaging. Slowest is 15
#define PPM_RX_DEBOUNCE			5		// debouncing interval, in samples
#define PPM_RX_PRECISE			1		// inter-frame filtering, on/off

#define PPM_TX_FRAME_LENGTH		225		// frame length in 0.1ms multiples
#define PPM_TX_PULSE_MIN		7		// minimal pulse length in 0.1ms
#define PPM_TX_PULSE_MAX		17		// maximal pulse length in 0.1ms
#define PPM_TX_PULSE_GAP		3		// pulse length in 0.1ms

#define PPM_TX_LOGIC_1			-20000	// output signal high level
#define PPM_TX_LOGIC_0			20000	// output signal low  level

enum
{
	PPM_RX_WAITSYNC_0    = -PPM_RX_PEAKS_TO_SYNC,
	PPM_RX_WAITSYNC_N    = -1,
	PPM_RX_WAITCHANNEL_0 = 0,	
};

enum
{
	PPM_TX_FRAMESYNC	= -1,
	PPM_TX_CHANNEL_0	= 0,
	PPM_TX_GAP_0		= PPM_TX_CHANNEL_0 + PPM_MAX_CHANNELS,	
};
// internal variables



// internal functions
short ppm_input_track(t_ppm_state *p_state, const short in);

// function definitions
void ppm_reset(t_ppm_state *p_state)
{
	assert(p_state != 0);

	memset(p_state, 0, sizeof(t_ppm_state));


	p_state->rx.state = PPM_RX_WAITSYNC_0;
	p_state->tx.state = PPM_TX_FRAMESYNC;
	p_state->max_smp = -32768;
	p_state->min_smp = 32767;
	p_state->last_smp = 32767; // to avoid false edge detection at the start
	p_state->rx.time_scale = 10000;
}

void ppm_set_tx(t_ppm_state *p_state, const unsigned short tenth_ms)
{
	assert(p_state != 0);
	p_state->tx.time_scale = tenth_ms;
}


short ppm_generate_sample(t_ppm_state *p_state)
{
	assert(p_state != 0);

	short smp = 0;
	
	const int channel = p_state->tx.state; // assuming that PPM_TX_CHANNEL_0 = 0
	
	p_state->tx.step_len++;

	if (channel >= PPM_TX_GAP_0)
	{
		if (p_state->tx.step_len < p_state->tx.time_scale * (unsigned int)PPM_TX_PULSE_GAP)
		{
			smp = PPM_TX_LOGIC_1;
		}
		else
		{
			p_state->tx.state -= PPM_MAX_CHANNELS;	//switch from gap to channel value
			if (p_state->tx.state >= p_state->tx.num_channels)
			{
				p_state->tx.state = PPM_TX_FRAMESYNC;
			}

			p_state->tx.step_len = 0;
			smp = PPM_TX_LOGIC_0;			
		}
	} // >= PPM_TX_GAP_0
	else if (channel >= PPM_TX_CHANNEL_0)
	{

		const unsigned int pulse_width = p_state->tx.time_scale * PPM_TX_PULSE_MIN 
			                         + ((p_state->tx.time_scale * (PPM_TX_PULSE_MAX - PPM_TX_PULSE_MIN) * (int)p_state->tx.value[channel]) >> 7);

		if (p_state->tx.step_len < pulse_width)
		{
			smp = PPM_TX_LOGIC_0;
		}
		else
		{
			p_state->tx.state++;					// advance to the next channel
			p_state->tx.state += PPM_MAX_CHANNELS;	// switch from channel value to gap
			
			p_state->tx.step_len = 0;
			smp = PPM_TX_LOGIC_1;
		}
	
	} // >= PPM_TX_CHANNEL_0
	else // p_state->tx.state == PPM_TX_FRAMESYNC
	{
		if (p_state->smp_txed < (int)PPM_TX_FRAME_LENGTH * p_state->tx.time_scale)
		{
			smp = PPM_TX_LOGIC_0;
		}
		else
		{
			p_state->tx.state = PPM_TX_GAP_0;
			p_state->smp_txed = -1;	

			p_state->tx.step_len = 0;
			smp = PPM_TX_LOGIC_1;
		}
	} // == PPM_TX_FRAMESYNC


	p_state->smp_txed++;

	return smp;

}

void ppm_process_sample(t_ppm_state *p_state, const short in)
{
	assert(p_state != 0);

	// calculate input metrics and center around zero
	short smp = ppm_input_track(p_state, in);

	p_state->rx.step_len++; // hope it won't overflow

	if (p_state->swing > PPM_RX_LEVEL_THRESHOLD)
	{
		
		// detect zero crossing
		if ((p_state->last_smp < 0) && (smp >= 0))
		{
			p_state->peak = p_state->rx.step_len; // for logging

			if (p_state->rx.step_len > PPM_RX_DEBOUNCE)
			{
				if (p_state->rx.state >= PPM_RX_WAITSYNC_N)
				{
					const int channel = p_state->rx.state; // assuming that PPM_WAITCHANNEL_0 = 0
					if (p_state->rx.step_len > (unsigned int)PPM_RX_FRAME_GAP * p_state->rx.time_scale)
					{
						// looks like a begining of a new frame and we are ready to sync
						p_state->rx.num_channels = channel;
						p_state->rx.state = PPM_RX_WAITCHANNEL_0; 						
					}
					else
					{
						if (0 <= channel && channel < PPM_MAX_CHANNELS)
						{
							
#if	PPM_RX_PRECISE
							// some filtering...
							// y[n] = (y[n-1] + x[n]) / 2
							
							// and input scaling by A to increase the precision:
							// p[n] = y[n] * A
							// p[n] = (p[n-1] + A * x[n]) / 2

							// +1 to correctly round-off the LSB
							// p[n] = (p[n-1] + A * x[n] + 1) / 2
							// A = 2
							p_state->rx.value[channel] += (p_state->rx.step_len << 1) + 1;
							p_state->rx.value[channel] >>= 1;
#else
							// maintain the scaling by A to preserve the calibration
							p_state->rx.value[channel] = p_state->rx.step_len << 1;
#endif
							// move on to the next channel
							p_state->rx.state++;
						}
					}						
				}
				else
				{					
					// accumulate sync info
					p_state->rx.state++;
				}


				if (p_state->rx.step_len < p_state->rx.time_scale)
				{
					p_state->rx.time_scale = p_state->rx.step_len;
				}

			}
			
			p_state->rx.step_len = 0;
		} // zero crossing
		else
		{
			p_state->peak = 0; // for logging only
		}


	} // input signal above threshold
	else
	{
		p_state->peak = 0; // for logging only
	}

	if (p_state->rx.step_len > (unsigned int)PPM_RX_FRAME_TIMEOUT * p_state->rx.time_scale)
	{
		// frame timeout, resync
		p_state->rx.state = PPM_RX_WAITSYNC_0;
		p_state->rx.time_scale = 10000;
		p_state->rx.num_channels = 0;
	}

	p_state->last_smp = smp;

}
bool ppm_has_framesync(t_ppm_state *p_state)
{
	assert(p_state != 0);

	return (p_state->rx.state >= PPM_RX_WAITCHANNEL_0);
}
int ppm_get_rx_channels(t_ppm_state *p_state)
{
	assert(p_state != 0);
	
	return p_state->rx.num_channels;
}
int ppm_get_value(t_ppm_state *p_state, const int channel)
{
	assert(p_state != 0);

	if ((0 <= channel) && (channel < PPM_MAX_CHANNELS))
	{
		return (p_state->rx.value[channel]);
	}
	else
	{
		return 0;
	}
}
void ppm_set_values(t_ppm_state *p_state, const short *values, const int count)
{
	assert(p_state != 0);
	assert(values != 0);

	assert(count >= 0);
	assert(count < PPM_MAX_CHANNELS);

	for (int i = 0; i < count; i++)
	{
		assert(values[i] >= 0);
		assert(values[i] <= 128);

		p_state->tx.value[i] = values[i];
	}
	p_state->tx.num_channels = count;

}
short ppm_input_track(t_ppm_state *p_state, const short in)
{
	int in2  = in >> 1;							// scale, so the a+b and a-b still fits into 'short'
	int in_p = in << (PPM_RX_INPUT_AVG - 1);	// scale to increase the averaging precision, = in2 << PPM_INPUT_AVG

	// positive peak detector
	if (in_p > p_state->max_smp)
	{
		p_state->max_smp = in_p;
	}
	else
	{
		p_state->max_smp += (in2 - (p_state->max_smp >> PPM_RX_INPUT_AVG));
	}

	// negative peak detector
	if (in_p < p_state->min_smp)
	{
		p_state->min_smp = in_p;
	}
	else
	{
		p_state->min_smp += (in2 - (p_state->min_smp >> PPM_RX_INPUT_AVG));
	}



	// threshold = 0.5 * (min + max)
	p_state->thresh = (p_state->max_smp + p_state->min_smp) >> (PPM_RX_INPUT_AVG + 1);

	// swing = max - min
	p_state->swing = (p_state->max_smp - p_state->min_smp) >> PPM_RX_INPUT_AVG;

	// center around the threshold
	short smp = (short)(in2 - p_state->thresh);


	return smp;
}


